#include "qr.h"
#include "householder.h"
#include <cmath>

#include <iostream>

#define ERROR 0.000000001
#define MAX 1000
using namespace std;

QR::QR(Matrix &matrix, double error): error(error), matrix(matrix){
	this->x = Matrix(matrix.getRow(),matrix.getColumn(),IDENTITY_MATRIX);
	this->lambda = Matrix(matrix.getRow(),matrix.getColumn(),IDENTITY_MATRIX);
}

QR::~QR(){
}

void QR::calculate(){//sem householder
	x = Matrix(matrix.getRow(),matrix.getColumn(),IDENTITY_MATRIX);	//  x - auto vetores

	//metodo de qr em si
	Matrix current = Matrix(matrix);
	Matrix r = Matrix(matrix);	//  r ~ triangular superior
	Matrix jt, qt;			//  produtorio(jt[i][j]) = qt
	double theta = 0.0, acum = 0.0;
	int k = 0;

	do{//iteracao k
		//iniciando sempre a Qtransposta do passo k com a identidade
		qt = Matrix(matrix.getRow(),matrix.getColumn(),IDENTITY_MATRIX);

		for (unsigned int j = 0; j < matrix.getColumn() - 1 ; j++){		// coluna
  			for (unsigned int i = j + 1; i < matrix.getRow(); i++){ 	// linha
				//iniciando smepre a matriz de jacobi com a identidade
				jt = Matrix(matrix.getRow(),matrix.getColumn(),IDENTITY_MATRIX);

				//calculando teta
				theta = std::atan(current[i][j]/current[j][j]);
				//gambiarra!
				if (current[j][j] == 0.0)
					theta = 0.0;

				//preenchendo a matriz de jacobi como matriz de rotação
				jt[i][i] = cos(theta);
				jt[j][j] = cos(theta);
				jt[i][j] = -sin(theta);
				jt[j][i] = sin(theta);

				//calculo do r
				r = jt * r;

				//calculo do q transposto
				qt = jt * qt;

  			}
		}//ao final tem o rk e o qtk

		//Ak+1 = rk*qk
		current = r * qt.transpose();

		//preparando r
		r = current;

		//atualizando a matriz dos autovetores
		x = x * qt.transpose();
		acum = 0.0;
		for (unsigned int j = 0; j < matrix.getColumn() - 1 ; j++)	// coluna
			for (unsigned int i = j+1; i < matrix.getRow(); i++)	// linha
				acum+= current[i][j];
		lambda = current;

        k++;
	}while(std::fabs(acum)>this->error && k < MAX);

	for(unsigned int i = 0; i < x.getRow(); i++){
        for(unsigned int j = 0; j < x.getColumn(); j++){
            if (fabs(x[i][j]) < ERROR)
                x[i][j] = 0.0;
        }
	}
}

/*

void QR::calculate(){//com householder
	x = Matrix(matrix.getRow(),matrix.getColumn(),IDENTITY_MATRIX);	//  x - auto vetores

	//utilizando householder
	Householder method;
	matrix = method.calculate(matrix, x);

	//metodo de qr em si
	Matrix current = Matrix(matrix);
	Matrix r = Matrix(matrix);	//  r ~ triangular superior
	Matrix jt, qt;			//  produtorio(jt[i][j]) = qt
	double theta = 0.0, acum = 0.0;

	do{//iteracao k
		//iniciando sempre a Qtransposta do passo k com a identidade
		qt = Matrix(matrix.getRow(),matrix.getColumn(),IDENTITY_MATRIX);

		for (unsigned int j = 0; j < matrix.getColumn() - 1 ; j++){		// coluna
			unsigned int i = j + 1;
			//iniciando smepre a matriz de jacobi com a identidade
			jt = Matrix(matrix.getRow(),matrix.getColumn(),IDENTITY_MATRIX);

			//calculando teta
			theta = std::atan(current[i][j]/current[j][j]);
			//gambiarra!
			if (current[j][j] == 0.0)
				theta = 0.0;

			//preenchendo a matriz de jacobi como matriz de rotação
			jt[i][i] = cos(theta);
			jt[j][j] = cos(theta);
			jt[i][j] = -sin(theta);
			jt[j][i] = sin(theta);

			//calculo do r
			r = jt * r;

			//calculo do q transposto
			qt = jt * qt;
		}//ao final tem o rk e o qtk

		//Ak+1 = rk*qk
		current = r * qt.transpose();

		//preparando r
		r = current;

		//atualizando a matriz dos autovetores
		x = x * qt.transpose();
		acum = 0.0;
		for (unsigned int j = 0; j < matrix.getColumn() - 1 ; j++)	// coluna
			acum+= current[j+1][j];
		lambda = current;

	}while(std::abs(acum)>this->error);

	for(unsigned int i = 0; i < x.getRow(); i++){
        for(unsigned int j = 0; j < x.getColumn(); j++){
            if (fabs(x[i][j]) < ERROR)
                x[i][j] = 0.0;
        }
	}
}

*/

void QR::printLambda(){
	lambda.print();
}

void QR::printVector(){
	x.print();
}

Matrix QR::getLambda(){
    Matrix result(lambda);
    for(unsigned int i = 0; i < lambda.getRow(); i++){
        for(unsigned int j = 0; j < lambda.getColumn(); j++){
            if(i != j) result[i][j] = 0.0;
        }
    }

	return result;
}

Matrix QR::getVector(){
	return x;
}

